/**
 * 
 */
package us.oh.k12.wkw.robot.subsystem;

import us.oh.k12.wkw.robot.command.LauncherAngleWithJoystickCmd;
import us.oh.k12.wkw.robot.model.FilterSmoothing;
import us.oh.k12.wkw.robot.util.WkwDashboard;
import us.oh.k12.wkw.robot.util.WkwPrefs;

import com.sun.squawk.util.MathUtils;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_I2C.AllAxes;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.tables.ITable;

/**
 * @author Team4145
 * 
 */
public class LaunchFrisbeeSystem extends SystemBase {

	protected static final String SYSTEM_NAME = "Launcher";

	private static final boolean ARCADE_DRIVE_ENABLED = true;

	private static final String NETWORKTABLE_NAME_KINECT_COORDINATES = "BFR_COORDINATES";
	private static final String NETWORKTABLE_NAME_KINECT_DISTANCE = "Distance";

	private static final double INITIAL_SHOOTER_MOTOR_SPEED = (double) -0.5;

	// ball shooter motors
	private Talon shootMotor1;
	private Talon shootMotor2;
	private Talon angleMotor;
	private DigitalInput topAngleSwitch;
	private DigitalInput bottomAngleSwitch;
	private Solenoid plungerRelayExtend;
	private Solenoid plungerRelayRetract;
	// private Compressor compressor;
	// private Relay compressorSpikeRelay;
	// private DigitalInput pressureGauge;

	// private Accelerometer accelorometer = null;
	// private I2C accel = null;
	private ADXL345_I2C adxl = null;

	private boolean angleMotorIsOn = false;
	private double angleMotorUpSpeed;
	private double angleMotorDownSpeed;
	private double angleMotorHoldSpeed;
	private long angleMotorOnStartTime = 0;
	private long angleMotorTotalOnTime = 0;
	private double previousAngleMotorSpeed = 0;
	private boolean topSwitchOn = false;
	private boolean bottomSwitchOn = false;

	private boolean shootMotor1IsOn = false;
	private boolean shootMotor2IsOn = false;
	// current motor speed (when we turn it on).
	private double shootMotor1Speed;
	private double shootMotor2Speed;
	// tracking how long the motor has been on.
	private long shootMotor1OnStartTime = 0;
	private long shootMotor2OnStartTime = 0;

	private long shootMotor1TotalOnTime = 0;
	private long shootMotor2TotalOnTime = 0;

	private int plungerFireCount = 0;
	private boolean plungerExtended = false;

	private double previousShootMotor1Speed = 0;
	private double previousShootMotor2Speed = 0;

	private double batteryVoltage = 0;

	private double targetCenterX = 0;
	private double targetCenterY = 0;

	private FilterSmoothing filterX = null;
	private FilterSmoothing filterY = null;
	private FilterSmoothing filterZ = null;

	private int delay = 0;

	private Joystick shooterJoystick;
	private RobotDrive drive; // Max P playing with joystick to angle launcher

	public LaunchFrisbeeSystem() {
		super("LauncherSystem");

		try {

			this.debug("LaunchFrisbeeSystem()", "Started.");

			this.initMotor();

			this.initAngleMotor();

			this.plungerRelayExtend = new Solenoid(1);
			this.plungerRelayRetract = new Solenoid(2);
			this.plungerRelayExtend.set(false);
			this.plungerRelayRetract.set(false);

			// this.compressor = new Compressor(2, 8);
			// this.compressor.start();
			// this.compressorSpikeRelay = new DigitalOutput(5);
			// this.pressureGauge = new DigitalInput(6);

			this.topAngleSwitch = new DigitalInput(9);
			this.topAngleSwitch = new DigitalInput(10);

			this.initAccelorometer();

			this.debug("LaunchFrisbeeSystem()", "Ended.");

		} catch (Exception anEx) {
			this.error("LaunchFrisbeeSystem()", anEx);
		}
	}

	public Joystick joystick = null;

	public void setJoystick(Joystick pJoystick) {
		this.joystick = pJoystick;
	}

	public boolean isAtTop() {
		return this.topAngleSwitch.get();
	}

	public boolean isAtBottom() {
		return this.bottomAngleSwitch.get();
	}

	private void initAccelorometer() {

		this.adxl = new ADXL345_I2C(DigitalInput.getDefaultDigitalModule(),
				ADXL345_I2C.DataFormat_Range.k2G);

		this.filterX = new FilterSmoothing(FilterSmoothing.TYPE_CIRCULAR,
				FilterSmoothing.DEFAULT_CIRCULAR_SIZE, FilterSmoothing.DEFAULT_FACTOR);
		this.filterY = new FilterSmoothing(FilterSmoothing.TYPE_CIRCULAR,
				FilterSmoothing.DEFAULT_CIRCULAR_SIZE, FilterSmoothing.DEFAULT_FACTOR);
		this.filterZ = new FilterSmoothing(FilterSmoothing.TYPE_CIRCULAR,
				FilterSmoothing.DEFAULT_CIRCULAR_SIZE, FilterSmoothing.DEFAULT_FACTOR);

	}

	private void initAngleMotor() {

		this.angleMotor = new Talon(3);

		if (null == this.angleMotor) {

			this.error("initMotor()", "Shoot motor not found.", null);

		} else {

			// initialize the motor.

			this.angleMotor.setExpiration(2.000000);
			this.angleMotor.setSafetyEnabled(false); // TODO: change to true
			this.angleMotor.set(0.000000); // make sure the motor is turned off.
			this.angleMotor.stopMotor();

			this.angleMotorIsOn = false;
			// set the initial motor speed.
			this.angleMotorUpSpeed = 0.2;

			this.angleMotorDownSpeed = 0.15;
			this.angleMotorHoldSpeed = .01;
			// reset the on time.
			this.angleMotorOnStartTime = 0;
			this.angleMotorTotalOnTime = 0;

			WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

			this.drive = new RobotDrive(this.angleMotor, this.angleMotor);

			if (null == this.drive) {

				this.error("initDrive()", "drive is null.", null);

			} else {

				// initialize the drive.

				this.drive.setExpiration(WkwPrefs.getDriveWatchdogTimeout());
				this.drive.setSafetyEnabled(WkwPrefs.isRobotWatchdogEnabled());

			}
		}
	}

	private void initMotor() {

		// setup the ball shooter jaguar and motor.

		this.shootMotor1 = new Talon(WkwPrefs.getShooterMotor1ControllerChannel());
		this.shootMotor2 = new Talon(WkwPrefs.getShooterMotor2ControllerChannel());

		// LiveWindow.addActuator("ShooterSystem", "shootMotor",
		// this.shootMotor);

		if (null == this.shootMotor1) {

			this.error("initMotor()", "Shoot motor 1 not found.", null);

		} else if (null == this.shootMotor2) {
			this.error("initMotor()", "Shoot motor 2 not found.", null);
		} else {

			// initialize the motor.

			this.shootMotor1.setExpiration(2.000000);
			this.shootMotor1.setSafetyEnabled(false); // TODO: change to true
			this.shootMotor1.set(0.000000); // make sure the motor is turned off.
			this.shootMotor1.stopMotor();
			this.shootMotor2.setExpiration(2.000000);
			this.shootMotor2.setSafetyEnabled(false); // TODO: change to true
			this.shootMotor2.set(0.000000); // make sure the motor is turned off.
			this.shootMotor2.stopMotor();
			this.shootMotor1IsOn = false;
			this.shootMotor2IsOn = false;
			// set the initial motor speed.
			this.shootMotor1Speed = LaunchFrisbeeSystem.INITIAL_SHOOTER_MOTOR_SPEED;
			this.shootMotor2Speed = LaunchFrisbeeSystem.INITIAL_SHOOTER_MOTOR_SPEED;

			// reset the on time.
			this.shootMotor1OnStartTime = 0;
			this.shootMotor2OnStartTime = 0;

			this.shootMotor1TotalOnTime = 0;
			this.shootMotor2TotalOnTime = 0;

			WkwDashboard.setShooterSpeed(this.shootMotor1.getSpeed());
			WkwDashboard.setShooterSpeed(this.shootMotor2.getSpeed());

		}

	}

	protected void initDefaultCommand() {

		try {

			if (LaunchFrisbeeSystem.ARCADE_DRIVE_ENABLED) {

				this.debug("initDefaultCommand()", "DriveWithJoystickCmd.");
				this.setDefaultCommand(new LauncherAngleWithJoystickCmd());
			}

			// no implementation.

		} catch (Exception anEx) {
			this.error("initDefaultCommand()", anEx);
		}
	}

	public void updateStatus() {

		try {

			this.angleMotor.Feed();
			this.shootMotor1.Feed();
			this.shootMotor2.Feed();

			if (null != this.shootMotor1) {

				if (Math.abs(this.previousShootMotor1Speed - this.shootMotor1.getSpeed()) > .001) {
					this.previousShootMotor1Speed = this.shootMotor1.getSpeed();
					WkwDashboard.setShooterSpeed(this.shootMotor1.getSpeed());
				}

			}
			if (null != this.shootMotor2) {

				if (Math.abs(this.previousShootMotor2Speed - this.shootMotor2.getSpeed()) > .001) {
					this.previousShootMotor2Speed = this.shootMotor2.getSpeed();
					WkwDashboard.setShooterSpeed(this.shootMotor2.getSpeed());
				}

			}

			if (null != this.angleMotor) {

				if (Math.abs(this.previousAngleMotorSpeed - this.angleMotor.getSpeed()) > .001) {
					this.previousAngleMotorSpeed = this.angleMotor.getSpeed();
					WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());
				}

			}

			if (Math.abs(this.batteryVoltage - DriverStation.getInstance().getBatteryVoltage()) > .01) {
				this.batteryVoltage = DriverStation.getInstance().getBatteryVoltage();
				WkwDashboard.setBatteryVolts(DriverStation.getInstance().getBatteryVoltage());
			}

			if (null != this.topAngleSwitch) {
				if (this.topSwitchOn != this.topAngleSwitch.get()) {
					this.topSwitchOn = this.topAngleSwitch.get();
					WkwDashboard.setTopSwitchState(this.topSwitchOn);
				}
			}

			if (null != this.bottomAngleSwitch) {
				if (this.bottomSwitchOn != this.bottomAngleSwitch.get()) {
					this.bottomSwitchOn = this.bottomAngleSwitch.get();
					WkwDashboard.setBottomSwitchState(this.bottomSwitchOn);
				}
			}

			if (this.plungerExtended != this.getPlungerState()) {
				this.plungerExtended = this.getPlungerState();
				WkwDashboard.setPlungerState(this.plungerExtended);
			}

			// this.debug("UpdateStatus", "Pressure Gauge = " + this.pressureGauge.get());

			WkwDashboard.setPlungerFireCount(this.plungerFireCount);

			// note: not sending setShooterMotorTotalOnTime() here.

			this.readAccel();

			this.calculateTargetCenter();

		} catch (Exception anEx) {
			this.error("updateStatus()", anEx);
		}
	}

	public void angleWithJoystick(final Joystick pJoystick) {

		try {

			// make sure we have the drive object.

			if (null != this.drive) {

				this.shooterJoystick = pJoystick;

				this.drive.arcadeDrive(pJoystick);

			}

		} catch (Exception anEx) {
			this.error("angleWithJoystick()", anEx);
		}
	}

	/*if (null != this.joystick) {

		double aValue = this.joystick.getThrottle();

		if (aValue > 0) {

			this.turnOnDownAngle();

		}

		else if (aValue < 0) {

			this.turnOnUpAngle();

		}
	}*/

	private void readAccel() {

		if (this.delay-- < 0) {

			this.delay = 20;

			if (null != this.adxl) {

				AllAxes anAllAxes = this.adxl.getAccelerations();

				if (null != anAllAxes) {

					double x = this.filterX.filter(anAllAxes.XAxis);
					double y = this.filterY.filter(anAllAxes.YAxis);
					double z = this.filterZ.filter(anAllAxes.ZAxis);
					double angle = this.computeAngle(x, y, z);

					WkwDashboard.setAccelX(x);
					WkwDashboard.setAccelY(y);
					WkwDashboard.setAccelZ(z);

					this.debug(
							"readAccel()",
							"X="
									+ FilterSmoothing.formatDouble(x, 2, 6)
									+ ", Y="
									+ FilterSmoothing.formatDouble(y, 2, 6)
									+ ", Z="
									+ FilterSmoothing.formatDouble(z, 2, 6)
									+ ", pitch="
									+ FilterSmoothing.formatDouble(MathUtils.asin(anAllAxes.YAxis),
											2, 6) + ", angle="
									+ FilterSmoothing.formatDouble(angle, 2, 4) + ".");

				}
			}
		}
	}

	private double computeAngle(double pX, double pY, double pZ) {
		double aXsq = pX * pX;
		double aYsq = pY * pY;
		double aDeviation = Math.sqrt(aXsq + aYsq);
		return MathUtils.atan(aDeviation / pZ);
	}

	private void calculateTargetCenter() {

		// ITable aTable =
		// NetworkTable.getTable(LaunchFrisbeeSystem.NETWORKTABLE_NAME_KINECT_COORDINATES);
		// aTable.retrieveValue(LaunchFrisbeeSystem.NETWORKTABLE_NAME_KINECT_COORDINATES,
		// arg1)
		// double aTopLeftX
		// ITable aTable = NetworkTable
		// .getTable(LaunchFrisbeeSystem.NETWORKTABLE_NAME_KINECT_COORDINATES);

		// if (null != aTable) {

		// this.debug("calculateTargetCenter()", "aTable=" + aTable.toString()
		// + ".");

		// }

		ITable aDashboardTable = NetworkTable.getTable("SmartDashboard");

		if (null != aDashboardTable) {

			/*
			 * try {
			 * 
			 * Object aCoordValue = aDashboardTable.getValue("target_coord");
			 * 
			 * if (null != aCoordValue) {
			 * 
			 * this.debug("calculateTargetCenter()", "aCoordValue=" +
			 * aCoordValue.getClass().getName() + ", aCoordValue=" + aCoordValue
			 * + ".");
			 * 
			 * } } catch (TableKeyNotDefinedException aTkndEx) { this.error(
			 * "calculateTargetCenter()", "TableKeyNotDefinedException message="
			 * + aTkndEx.getMessage() + ".", null); }
			 */
			/*
			 * // Object anObject = null; Double[] anObject = null;
			 * aDashboardTable.retrieveValue("target_coord", anObject);
			 * 
			 * this.debug("calculateTargetCenter()", "anObject=" +
			 * anObject.getClass().getName() + ", anObject=" +
			 * anObject.toString() + ".");
			 * 
			 * try {
			 * 
			 * NumberArray aNumberArray = (NumberArray) aCoordValue;
			 * 
			 * this.debug("calculateTargetCenter()", "aNumberArray=" +
			 * aNumberArray.getClass().getName() + ", aNumberArray=" +
			 * aNumberArray + ".");
			 * 
			 * } catch (ClassCastException aCcEx) {
			 * this.error("calculateTargetCenter()", aCcEx); }
			 */

			// try {

			/*
			 * double aTopLeftX = aDashboardTable.getNumber("tlx"); double
			 * aTopLeftY = aDashboardTable.getNumber("tly"); double
			 * aTopRightX = aDashboardTable.getNumber("trx"); double
			 * aTopRightY = aDashboardTable.getNumber("try");
			 * 
			 * double aBottomLeftX = aDashboardTable.getNumber("blx");
			 * double aBottomLeftY = aDashboardTable.getNumber("bly");
			 * double aBottomRightX = aDashboardTable.getNumber("brx");
			 * double aBottomRightY = aDashboardTable.getNumber("bry");
			 */

			// double aCenterX = aDashboardTable.getNumber("x_center");
			// double aCenterY = aDashboardTable.getNumber("y_center");
			/*
			 * Object aDistanceValue = aDashboardTable.getValue("Distance");
			 * 
			 * if (null != aDistanceValue) {
			 * 
			 * this.debug("calculateTargetCenter()", "aDistanceValue=" +
			 * aDistanceValue.getClass().getName() + ", aDistanceValue=" +
			 * aDistanceValue + ".");
			 * 
			 * }
			 */

			// this.debug("calculateTargetCenter()", "aCenterX=" + aCenterX + ", aCenterY="
			// + aCenterY + ".");

			// this.debug("calculateTargetCenter()", "aTopLeftX=" +
			// aTopLeftX
			// + ", aTopLeftY=" + aTopLeftY + ".");

			// this.debug("calculateTargetCenter()", "aTopRightX="
			// + aTopRightX + ", aBottomLeftY=" + aBottomLeftY + ".");
			// this.debug("calculateTargetCenter()", "aBottomLeftX="
			// + aBottomLeftX + ", aTopLeftX=" + aTopLeftX + ".");
			// this.debug("calculateTargetCenter()", "aBottomRightX="
			// + aBottomRightX + ", aBottomRightY=" + aBottomRightY
			// + ".");

			// } catch (TableKeyNotDefinedException aTkndEx) {
			// this.error("calculateTargetCenter()", "TableKeyNotDefinedException message="
			// + aTkndEx.getMessage() + ".", null);
			// }

		}

	}

	public void plungerFire() {
		this.plungerRelayRetract.set(false);
		this.debug("plungerFire()", "Called");
		this.plungerRelayExtend.set(true);
		this.plungerFireCount++;
		this.plungerExtended = true;
	}

	public void plungerRetract() {

		this.plungerRelayExtend.set(false);
		this.debug("plungerRetract()", "Called");
		this.plungerRelayRetract.set(true);
		this.plungerExtended = false;
	}

	public boolean getPlungerState() {
		return Relay.Value.kOn.equals(this.plungerRelayExtend);
	}

	/*public void setCompressorOn(boolean pOn) {
		if (pOn) {
			this.compressor.setRelayValue(Relay.Value.kOn);
		} else {
			this.compressor.setRelayValue(Relay.Value.kOff);
		}
	}

	public void setCompressorEnabled(boolean pEnabled) {
		if (pEnabled) {
			this.compressor.setRelayValue(Relay.Value.kOn);
		} else {
			this.compressor.setRelayValue(Relay.Value.kOff);
		}
	}*/

	public void turnOnUpAngle() {

		try {

			// make sure we have the motor object

			if (null != this.angleMotor) {
				if (!this.angleMotorIsOn) {
					this.angleMotor.set(this.angleMotorUpSpeed);

					this.angleMotorIsOn = true;

					WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

					this.angleMotorOnStartTime = System.currentTimeMillis();
				}
			}

		} catch (Exception anEx) {
			this.error("turnOnUpAngle()", anEx);
		}
	}

	public void turnOnHoldAngle() {

		try {

			// make sure we have the motor object

			if (null != this.angleMotor) {

				this.angleMotor.set(this.angleMotorHoldSpeed);

				this.angleMotorIsOn = true;

				WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

				this.angleMotorOnStartTime = System.currentTimeMillis();

			}

		} catch (Exception anEx) {
			this.error("turnOnUpAngle()", anEx);
		}
	}

	public void stopChangingAngle() {

		try {

			// make sure we have a motor object.

			if (null != this.angleMotor) {
				if (this.angleMotorIsOn) {
					this.debug("stopChangingAngle()", "Called.");

					this.angleMotor.set(0.00000000); // set speed at zero

					this.angleMotor.stopMotor(); // stop the motor

					this.angleMotorIsOn = false;

					WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

					this.angleMotorTotalOnTime += System.currentTimeMillis()
							- this.angleMotorOnStartTime;
					this.angleMotorOnStartTime = 0;

					WkwDashboard.setShooterMotorTotalOnTime(this.angleMotorTotalOnTime);
				}
			}

		} catch (Exception anEx) {
			this.error("stopChangingAngle()", anEx);
		}
	}

	public void turnOnDownAngle() {
		try {

			// make sure we have the motor object

			if (null != this.angleMotor) {
				if (!this.angleMotorIsOn) {
					this.angleMotor.set(-this.angleMotorDownSpeed);

					this.angleMotorIsOn = true;

					WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

					this.angleMotorOnStartTime = System.currentTimeMillis();
				}
			}

		} catch (Exception anEx) {
			this.error("turnOnDownAngle()", anEx);
		}
	}

	public void setAngleMotorSpeed(final double pSpeed) {

		try {

			double aSpeed = pSpeed;

			this.debug("setAngleMotorSpeed()", "aSpeed=" + aSpeed + ".");

			if (aSpeed < -1.000000) {
				aSpeed = -1.000000;
			} else if (aSpeed > 1.000000) {
				aSpeed = 1.000000;
			}

			// save the new speed value.

			this.angleMotorUpSpeed = aSpeed;

			// Doesn't allow the motor to fire in reverse

			if (this.angleMotorUpSpeed > 0.000000) {
				this.angleMotorUpSpeed = 0.000000;
			}

			this.debug("setAngleMotorSpeed()", "angleMotorSpeed=" + this.angleMotorUpSpeed + ".");

			WkwDashboard.setAngleDesiredSpeed(this.angleMotorUpSpeed);

			// do we have a motor object and is the motor on?

			if ((null != this.angleMotor) && (this.angleMotorIsOn)) {

				// set the new speed in the motor.
				this.angleMotor.set(this.angleMotorUpSpeed);
				WkwDashboard.setAngleSpeed(this.angleMotor.getSpeed());

			}

		} catch (Exception anEx) {
			this.error("setAngleMotorSpeed()", anEx);
		}
	}

	/**
	 * Convenience method to increase the motor speed.
	 * 
	 * @param pSpeed
	 *            Offset from current speed.
	 */
	public void increaseAngleSpeed(final double pSpeed) {
		this.setAngleMotorSpeed(this.angleMotorUpSpeed - pSpeed);
	}

	/**
	 * Convenience method to decrease the angle motor speed.
	 * 
	 * @param pSpeed
	 *            Offset from current speed.
	 */
	public void decreaseAngleSpeed(final double pSpeed) {
		this.setAngleMotorSpeed(this.angleMotorUpSpeed + pSpeed);
	}

	/**
	 * How fast is the angle motor turning?
	 * 
	 * @return rpms.
	 */
	public double getAngleMotorSpeed() {
		return (null == this.angleMotor ? 0 : this.angleMotor.getSpeed());
	}

	/**
	 * Is the angle motor running?
	 * 
	 * @return true if angle motor is running.
	 */
	public boolean isAngleMotorOn() {
		return this.angleMotorIsOn;
	}

	public long getAngleMotorTotalOnTime() {
		return this.angleMotorTotalOnTime;
	}

	/**
	 * Turn on the shooter motor.
	 */
	public void turnOnShootMotor() {

		try {

			// make sure we have the motor object

			if (null != this.shootMotor1) {

				this.shootMotor1.set(this.shootMotor1Speed);

				this.shootMotor1IsOn = true;

				WkwDashboard.setShooterSpeed(this.shootMotor1.getSpeed());

				this.shootMotor1OnStartTime = System.currentTimeMillis();

			}

			if (null != this.shootMotor2) {

				this.shootMotor2.set(this.shootMotor2Speed);

				this.shootMotor2IsOn = true;

				WkwDashboard.setShooterSpeed(this.shootMotor2.getSpeed());

				this.shootMotor2OnStartTime = System.currentTimeMillis();

			}

		} catch (Exception anEx) {
			this.error("turnOnShootMotor1()", anEx);
		}
	}

	/**
	 * Turn off the shooter motor.
	 */
	public void turnOffShootMotor() {

		try {

			// make sure we have a motor object.

			if (null != this.shootMotor1) {

				this.shootMotor1.set(0.00000000); // set speed at zero

				this.shootMotor1.stopMotor(); // stop the motor

				this.shootMotor1IsOn = false;

				WkwDashboard.setShooterSpeed(this.shootMotor1.getSpeed());

				this.shootMotor1TotalOnTime += System.currentTimeMillis()
						- this.shootMotor1OnStartTime;
				this.shootMotor1OnStartTime = 0;

				WkwDashboard.setShooterMotorTotalOnTime(this.shootMotor1TotalOnTime);

			}

			if (null != this.shootMotor2) {

				this.shootMotor2.set(0.00000000); // set speed at zero

				this.shootMotor2.stopMotor(); // stop the motor

				this.shootMotor2IsOn = false;

				WkwDashboard.setShooterSpeed(this.shootMotor2.getSpeed());

				this.shootMotor2TotalOnTime += System.currentTimeMillis()
						- this.shootMotor2OnStartTime;
				this.shootMotor2OnStartTime = 0;

				WkwDashboard.setShooterMotorTotalOnTime(this.shootMotor2TotalOnTime);

			}

		} catch (Exception anEx) {
			this.error("turnOffShootMotor1()", anEx);
		}
	}

	/**
	 * Set the shoot motor speed. Note: this does NOT turn on the motor.
	 * 
	 * @param pSpeed
	 *            rpms (-1.0 to +1.0).
	 */
	public void setShootMotor1Speed(final double pSpeed) {

		try {

			double aSpeed = pSpeed;

			this.debug("setShootMotor1Speed()", "aSpeed=" + aSpeed + ".");

			if (aSpeed < -1.000000) {
				aSpeed = -1.000000;
			} else if (aSpeed > 1.000000) {
				aSpeed = 1.000000;
			}

			// save the new speed value.

			this.shootMotor1Speed = aSpeed;

			// Doesn't allow the motor to fire in reverse

			if (this.shootMotor1Speed > 0.000000) {
				this.shootMotor1Speed = 0.000000;
			}

			this.debug("setShootMotor1Speed()", "shootMotor1Speed=" + this.shootMotor1Speed + ".");

			WkwDashboard.setShooterDesiredSpeed(this.shootMotor1Speed);

			// do we have a motor object and is the motor on?

			if ((null != this.shootMotor1) && (this.shootMotor1IsOn)) {

				// set the new speed in the motor.
				this.shootMotor1.set(this.shootMotor1Speed);
				WkwDashboard.setShooterSpeed(this.shootMotor1.getSpeed());

			}

		} catch (Exception anEx) {
			this.error("setShootMotor1Speed()", anEx);
		}
	}

	public void setShootMotor2Speed(final double pSpeed) {

		try {

			double aSpeed = pSpeed;

			this.debug("setShootMotor2Speed()", "aSpeed=" + aSpeed + ".");

			if (aSpeed < -1.000000) {
				aSpeed = -1.000000;
			} else if (aSpeed > 1.000000) {
				aSpeed = 1.000000;
			}

			// save the new speed value.

			this.shootMotor2Speed = aSpeed;

			// Doesn't allow the motor to fire in reverse

			if (this.shootMotor2Speed > 0.000000) {
				this.shootMotor2Speed = 0.000000;
			}

			this.debug("setShootMotor2Speed()", "shootMotor2Speed=" + this.shootMotor2Speed + ".");

			WkwDashboard.setShooterDesiredSpeed(this.shootMotor2Speed);

			// do we have a motor object and is the motor on?

			if ((null != this.shootMotor2) && (this.shootMotor2IsOn)) {

				// set the new speed in the motor.
				this.shootMotor2.set(this.shootMotor2Speed);
				WkwDashboard.setShooterSpeed(this.shootMotor2.getSpeed());

			}

		} catch (Exception anEx) {
			this.error("setShootMotor2Speed()", anEx);
		}
	}

	/**
	 * Convenience method to increase the motor speed. This call the setShootMotorSpeed() method.
	 * 
	 * @param pSpeed
	 *            Offset from current speed.
	 */
	public void increaseMotor1Speed(final double pSpeed) {
		this.setShootMotor1Speed(this.shootMotor1Speed - pSpeed);
	}

	public void increaseMotor2Speed(final double pSpeed) {
		this.setShootMotor2Speed(this.shootMotor2Speed - pSpeed);
	}

	/**
	 * Convenience method to decrease the motor speed. This call the setShootMotorSpeed() method.
	 * 
	 * @param pSpeed
	 *            Offset from current speed.
	 */
	public void decreaseMotor1Speed(final double pSpeed) {
		this.setShootMotor1Speed(this.shootMotor1Speed + pSpeed);

	}

	public void decreaseMotor2Speed(final double pSpeed) {
		this.setShootMotor2Speed(this.shootMotor2Speed + pSpeed);

	}

	/**
	 * How fast is the shoot motor turning?
	 * 
	 * @return rpms.
	 */
	public double getShootMotor1Speed() {
		return (null == this.shootMotor1 ? 0 : this.shootMotor1.getSpeed());
	}

	public double getShootMotor2Speed() {
		return (null == this.shootMotor2 ? 0 : this.shootMotor2.getSpeed());
	}

	/**
	 * Is the shoot motor running?
	 * 
	 * @return true if shoot motor is running.
	 */
	public boolean isShootMotor1On() {
		return this.shootMotor1IsOn;
	}

	public boolean isShootMotor2On() {
		return this.shootMotor2IsOn;
	}

	public long getShootMotor1TotalOnTime() {
		return this.shootMotor1TotalOnTime;
	}

	public long getShootMotor2TotalOnTime() {
		return this.shootMotor2TotalOnTime;
	}

	/*
	private void nothing() {

		this.debug("initAccelorometer()",
				"getDefaultDigitalModule=" + DigitalInput.getDefaultDigitalModule() + ".");

		DigitalModule aDigitalModule = DigitalModule.getInstance(DigitalInput
				.getDefaultDigitalModule());

		if (null == aDigitalModule) {

			this.debug("initAccelorometer()", "aDigitalModule is null.");

		} else {

			this.debug("initAccelorometer()", "aDigitalModule.");

			int anAddress = 0x3a;

			// I2C aI2c = new I2C(aDigitalModule, anAddress);

			this.accel = aDigitalModule.getI2C(anAddress);

			if (null == this.accel) {

				this.debug("initAccelorometer()", "this.accel is null.");

			} else {

				this.debug("initAccelorometer()", "this.accel.");

				if (this.accel.addressOnly()) {

					this.debug("initAccelorometer()", "this.accel.addressOnly is true");

					// this.accel.setCompatabilityMode(true);

					int aRegisterAddress = 0x31;
					int aData = 0x08;

					this.accel.write(aRegisterAddress, aData);

					aRegisterAddress = 0x2d;
					aData = 0x08;

					this.accel.write(aRegisterAddress, aData);

				} else {

					this.debug("initAccelorometer()", "this.accel.addressOnly is false.");

				}

			}
		}
	}
	*/

	/*
	private double[] smoothArray = new double[10];
	private int smoothIndex = 0;

	private double smoothValue(final double pInValue) {

		this.smoothArray[this.smoothIndex++] = pInValue;

		if (this.smoothIndex == this.smoothArray.length) {
			this.smoothIndex = 0;
		}

		double aSum = 0;

		for (int idx = 0; idx < this.smoothArray.length; idx++) {
			aSum += this.smoothArray[idx];
		}

		return aSum / this.smoothArray.length;
	}
	*/

	/*
	private void readAccelOld() {

		int aXHiReg = 0x33;
		int aXLoReg = 0x32;
		int aYHiReg = 0x35;
		int aYLoReg = 0x34;
		int aZHiReg = 0x37;
		int aZLoReg = 0x36;

		int aX = this.readValue(aXHiReg, aXLoReg);
		int aY = this.readValue(aYHiReg, aYLoReg);
		int aZ = this.readValue(aZHiReg, aZLoReg);

		this.debug("readAccel()", "aX=" + aX + ", aY=" + aY + ", aZ=" + aZ + ".");
	}

	private int readValue(final int pHiReg, final int pLowReg) {
		byte[] aHighByte = new byte[1];
		byte[] aLowByte = new byte[1];

		this.accel.read(pHiReg, aHighByte.length, aHighByte);
		this.accel.read(pLowReg, aLowByte.length, aLowByte);

		return this.setupValue(aHighByte[0], aLowByte[0]);
	}
	*/
	/*
	private int setupValue(final byte pHighByte, final byte pLowByte) {
		int aInt = (int) pHighByte;

		int aHighInt = (aInt << 8) & 0xff00;

		int aLowInt = pLowByte & 0x00ff;

		return aHighInt | aLowInt;
	}
	*/

}
